_Pragma("once");
#include "ros/ros.h"
#include "cv_bridge/cv_bridge.h"
#include <opencv2/opencv.hpp>
#include "image_transport/image_transport.h"
#include "inference_engine.hpp"
#include <samples/ocv_common.hpp>
#include <ngraph/ngraph.hpp>
#include <samples/slog.hpp>
#include <iostream>
#include "YoloParams.h"

#define THRESOLD 0.3
#define CONFINDENCE_THRE 0.1
#define IOU_THRE 0.5
using namespace InferenceEngine;

class RosDetect
{
private:
    std::string class_name = "/home/user/Documents/yolov3_tiny_bike.txt";
    std::string net_work = "/home/user/Documents/yolov3_tiny_bicycle.xml";
    std::string input_name;
    std::string device_name = "CPU";
    int inference_num = 1;

    Blob::Ptr imgBlob;
    Blob::Ptr output;
    int inference_count = 0;
    cv::Mat src_img;
    ros::NodeHandle nh;
    image_transport::ImageTransport it;
    image_transport::Publisher pub_raw_image;     //原图
    image_transport::Publisher pub_colored_image; //语义图
    image_transport::Publisher pub_depth_image;   //深度图
    image_transport::Subscriber sub_raw_image;    //从Kinect接收原图像
    // image_transport::Subscriber sub_depth_image;    //从Kinect接收深度图像
    InferenceEngine::Core core; //用于设备管理和载入网络
    CNNNetwork network;         //读取网络

    std::vector<std::string> labels;              //标签
    std::vector<InferRequest::Ptr> request_queue; //创建请求队列
    std::vector<std_msgs::Header> header_vector;
    std::vector<cv::Mat> image_vec;                 //创建图片队列
    std::map<std::string, cv::Scalar> color_labels; //标签和颜色的映射
    InferenceEngine::InputsDataMap input_info;      //获取输入信息用于下面设置
    InferenceEngine::OutputsDataMap output_info;    //获取输出信息用于下面设置
    std::map<std::string, YoloParams> yoloParams;   //yolo输出参数

    ExecutableNetwork executable_network; //把网络加载进设备
    InferRequest infer_request;           //创建推理请求

public:
    RosDetect();
    RosDetect(std::string para);
    void getPara(std::string para);
    struct DetectionObject
    {
        int xmin, ymin, xmax, ymax, class_id;
        float confidence;

        DetectionObject(double x, double y, double h, double w, int class_id, float confidence, float h_scale, float w_scale)
        {
            this->xmin = static_cast<int>((x - w / 2) * w_scale);
            this->ymin = static_cast<int>((y - h / 2) * h_scale);
            this->xmax = static_cast<int>(this->xmin + w * w_scale);
            this->ymax = static_cast<int>(this->ymin + h * h_scale);
            this->class_id = class_id;
            this->confidence = confidence;
        }

        bool operator<(const DetectionObject &s2) const
        {
            return this->confidence < s2.confidence;
        }
        bool operator>(const DetectionObject &s2) const
        {
            return this->confidence > s2.confidence;
        }
    };

    void InferenceAndPub(const sensor_msgs::ImageConstPtr &msg);
    static int EntryIndex(int side, int lcoords, int lclasses, int location, int entry);
    double IntersectionOverUnion(const DetectionObject &box_1, const DetectionObject &box_2);
    void ParseYOLOV3Output(const YoloParams &params, const std::string &output_name,
                           const Blob::Ptr &blob, const unsigned long resized_im_h,
                           const unsigned long resized_im_w, const unsigned long original_im_h,
                           const unsigned long original_im_w,
                           const double threshold, std::vector<DetectionObject> &objects);
};
